import numpy as np


class FacePointDistance:
    def __init__(self, face, point):
        """
        Args:
            face:[[x1,y1,z1],[x2,y2,z2],[x3,y3,z3]]
            point:[x4,y4,z4]

        Returns:

        """
        self.face = face
        self.point = point

    def define_area(self):
        """
        法向量    ：n={A,B,C}
        空间上某点：p={x0,y0,z0}
        点法式方程：A(x-x0)+B(y-y0)+C(z-z0)=Ax+By+Cz-(Ax0+By0+Cz0)
        https://wenku.baidu.com/view/12b44129af45b307e87197e1.html
        :param point1:
        :param point2:
        :param point3:
        :param point4:
        :return:（Ax, By, Cz, D）代表：Ax + By + Cz + D = 0
        """
        point1, point2, point3 = self.face[0], self.face[1], self.face[2]
        point1 = np.asarray(point1)
        point2 = np.asarray(point2)
        point3 = np.asarray(point3)
        AB = np.asmatrix(point2 - point1)
        AC = np.asmatrix(point3 - point1)
        N = np.cross(AB, AC)  # 向量叉乘，求法向量
        # Ax+By+Cz
        Ax = N[0, 0]
        By = N[0, 1]
        Cz = N[0, 2]
        D = -(Ax * point1[0] + By * point1[1] + Cz * point1[2])
        return Ax, By, Cz, D

    def point2area_distance(self):
        """
        :param point1:数据框的行切片，三维
        :param point2:
        :param point3:
        :param point4:
        :return:点到面的距离
        """
        point4 = self.point
        Ax, By, Cz, D = self.define_area()
        mod_d = Ax * point4[0] + By * point4[1] + Cz * point4[2] + D
        mod_area = np.sqrt(np.sum(np.square([Ax, By, Cz])))
        d = abs(mod_d) / mod_area
        return d


if __name__ == '__main__':
    # 初始化数据
    point1 = [2, 3, 1]
    point2 = [4, 1, 2]
    point3 = [6, 3, 7]
    point4 = [-5, -4, 8]
    # 计算点到面的距离
    d1 = FacePointDistance(face=[point1, point2, point3], point=point4).point2area_distance()  # s=8.647058823529413

    print("点到面的距离s: " + str(d1))
